/*
 *  Contacts.cpp
 *  c_Render
 *
 *  Created by liao peterson on 2/22/10.
 *  Copyright 2010 uestc. All rights reserved.
 *
 */
#include"Physics.h"

contact::contact()
{
	contactPoint.SetZero();
	locContactPointOne.SetZero();
	locContactPointTwo.SetZero();

	contactNormal.SetZero();

	penetration = 0;
	restitution = CONTACT_BOUNCE;

	indexOne = 0;
	indexTwo = 0;
	
	one = NULL;
	two = NULL;

	contactVel.SetZero();
	//contact2World.SetZero();
	deltaVelocity = 0;
	desiredDeltaVelocity = 0;
	relativeContactPosition[0].SetZero();
	relativeContactPosition[1].SetZero();

	rotRes[0] = 0;
	rotRes[1] = 0;
	
	velChange[0].SetZero();
	angVelChange[0].SetZero();
	velChange[1].SetZero();
	angVelChange[1].SetZero();
}

void contact::calInternal()
{
	calContactTransform();
	deltaVelocity=0;
	if(one->pi.mass>STATIC_MASS)
		deltaVelocity += calDeltaVel(0);
	if(two->pi.mass>STATIC_MASS)
		deltaVelocity += calDeltaVel(1);
	
	calDesiredDeltaVel();
}

void contact::calContactTransform()
{
	// Construct the contact space basis, x axis(contact space) is the contact normal
	//check whether contact normal is closer to x axis or y axis(world space)
	if(fabs(contactNormal.x) > fabs(contactNormal.y)){
		//close to x axis, choose contact space y axis to be world y axis
		vec3 y = {0, 1.0, 0};
		vec3 z = contactNormal.cross(y);
		y = z.cross(contactNormal);
		contact2World.setComponent(contactNormal, y, z);
	}else{
		//close to y axis, choose contact space y axis to be world x axis
		vec3 y = {1.0, 0, 0};
		vec3 z = contactNormal.cross(y);
		y = z.cross(contactNormal);
		contact2World.setComponent(contactNormal, y, z);
	}
}

// index:0 for A, 1 for B
GLfloat contact::calDeltaVel(int index)
{
	physicsIntegrator*target = (index==0)?one:two;
	relativeContactPosition[index] = contactPoint - target->pi.loc;
	vec3 torquePerUnitImpulse = contactNormal.cross(relativeContactPosition[index]);
	vec3 rotationPerUnitImpulse = torquePerUnitImpulse / target->pi.uniMoment;
	vec3 velocityPerUnitImpulse = rotationPerUnitImpulse.cross(relativeContactPosition[index]);
	GLfloat deltaVelocity = fabs(velocityPerUnitImpulse * contactNormal);
	rotRes[index] = deltaVelocity;
	deltaVelocity += 1/target->pi.mass;
	return deltaVelocity;
}



void contact::calDesiredDeltaVel()
{
	// Step 2
	// Calculate the closing relative velocity
	//add the velocity of the second object to 
	contactVel = -one->pi.angVel.cross(relativeContactPosition[0]) + one->pi.vel;
	contactVel -= -two->pi.angVel.cross(relativeContactPosition[1]) + two->pi.vel;
	//calculate the relative closing velocity
	contactVel = contact2World.transpose() * contactVel;
	
	contactVel.x *= PENETRATION_SHRINKAGE;
	// Step 3
	// In the contact space calculate the desired velocity change.
	desiredDeltaVelocity = (contactVel.x * (1 + restitution));
}


GLboolean contact::Compare(contact*data)
{
	if((one->identity==data->one->identity)&(indexOne==data->indexOne)&
	   (two->identity==data->two->identity)&(indexTwo==data->indexTwo))
		return true;
	else if((one->identity==data->two->identity)&(indexOne==data->indexTwo)&
			(two->identity==data->one->identity)&(indexTwo==data->indexOne))
		return true;
	return false;
}

void contact::copyVecs(contact*data)
{
	one->setMatrix();
	two->setMatrix();
	
	contactPoint=data->contactPoint;
	contactNormal=data->contactNormal;
	penetration = data->penetration;
	originalPenetration = data->originalPenetration;
	estimatedPenetration = data->penetration;
	locContactPointOne = one->itransform * contactPoint;
	locContactPointTwo = two->itransform * contactPoint;
}

void contact::SetContact(vec3 cPoint,vec3 cNormal,GLfloat pen,GLint index1,GLint index2,physicsIntegrator*A,physicsIntegrator*B)
{
	//one->setMatrix();
	//two->setMatrix();
	
	contactPoint = cPoint;
	locContactPointOne = A->itransform * contactPoint;
	locContactPointTwo = B->itransform * contactPoint;
	
	contactNormal = cNormal;
	penetration = pen;
	originalPenetration = pen;
	estimatedPenetration = pen;
	
	one = A;
	two = B;
	//indexOne = (index>6)?((index-6)/3+1):((index>3)?-1:index);
	//indexTwo = (index>6)?((index-1)%3+1):((index>3)?index-3:-1);
	indexOne = index1;
	indexTwo = index2;
}

/*inline GLfloat transformToAxis(const physicsIntegrator &p, const vec3 &axis)
{
	box* bb = p.pi.bBox;
	return 		bb->halfSize.x * fabs(axis * p.getAxis(0)) +
	bb->halfSize.y * fabs(axis * p.getAxis(1)) + 
	bb->halfSize.z * fabs(axis * p.getAxis(2));
}

vec3 contact::getAxis()
{
	if(indexOne==-1)
		return two->getAxis(indexTwo-1);
	else if(indexTwo==-1)
		return one->getAxis(indexOne-1);
	else
		return one->getAxis(indexOne-1)%two->getAxis(indexTwo-1);
	
	//return NULL;
}*/

void contact::recalPen()
{
	one->setMatrix();
	two->setMatrix();
	vec3 contactA = one->transform * locContactPointOne;
	vec3 contactB = two->transform * locContactPointTwo;
	penetration = originalPenetration + (contactB - contactA) * contactNormal;
	estimatedPenetration = (contactB - contactA).norm() * (((contactB-contactA)*contactNormal>0)?1.0f:-1.0f);
}